function plot_comparison(true_traj, ekf_traj, graph_traj, fastslam_traj, grid_traj, ...
                         landmarks, ekf_lm, graph_lm, fastslam_result, grid_map, ...
                         ekf_stats, graph_stats, fastslam_stats, grid_stats)
% 算法对比可视化（包含Grid SLAM）

    figure('Position', [50, 50, 1800, 1200]);
    
    % 子图1: 所有算法轨迹对比
    subplot(4, 3, 1); hold on; grid on; axis equal;
    plot(landmarks(:, 1), landmarks(:, 2), 'k*', 'MarkerSize', 8, 'LineWidth', 1.5);
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2.5);
    plot(ekf_traj(1, :), ekf_traj(2, :), 'b--', 'LineWidth', 1.5);
    plot(graph_traj(1, :), graph_traj(2, :), 'r:', 'LineWidth', 1.5);
    plot(fastslam_traj(1, :), fastslam_traj(2, :), 'm-.', 'LineWidth', 1.5);
    plot(grid_traj(1, :), grid_traj(2, :), 'c--', 'LineWidth', 1.5);
    legend('真实路标', '真实轨迹', 'EKF', 'Graph', 'FastSLAM', 'Grid+MCL', 'Location', 'northeast');
    xlabel('X (m)'); ylabel('Y (m)');
    title('【总览】所有SLAM算法轨迹对比');
    xlim([-5, 50]); ylim([-5, 50]);
    
    % 子图2: EKF-SLAM轨迹与路标
    subplot(4, 3, 2); hold on; grid on; axis equal;
    plot(landmarks(:, 1), landmarks(:, 2), 'k*', 'MarkerSize', 8, 'LineWidth', 1.5);
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2, 'Color', [0.5 0.8 0.5]);
    plot(ekf_traj(1, :), ekf_traj(2, :), 'b-', 'LineWidth', 2);
    if isstruct(ekf_lm) && isfield(ekf_lm, 'positions') && ~isempty(ekf_lm.positions)
        plot(ekf_lm.positions(1, :), ekf_lm.positions(2, :), 'bo', 'MarkerSize', 6, 'LineWidth', 1.5);
    elseif ekf_lm.Count > 0
        lm_keys = cell2mat(keys(ekf_lm));
        est_lm = zeros(length(lm_keys), 2);
        for i = 1:length(lm_keys)
            lm_pos = ekf_lm(lm_keys(i));
            est_lm(i, :) = lm_pos';
        end
        plot(est_lm(:, 1), est_lm(:, 2), 'bo', 'MarkerSize', 6, 'LineWidth', 1.5);
    end
    legend('真实路标', '真实轨迹', 'EKF轨迹', 'EKF路标', 'Location', 'northeast');
    xlabel('X (m)'); ylabel('Y (m)');
    title(sprintf('EKF-SLAM (路标:%d, 误差:%.3fm)', ekf_stats.n_landmarks, ekf_stats.avg_error));
    xlim([-5, 50]); ylim([-5, 50]);
    
    % 子图3: Graph-SLAM轨迹与路标
    subplot(4, 3, 3); hold on; grid on; axis equal;
    plot(landmarks(:, 1), landmarks(:, 2), 'k*', 'MarkerSize', 8, 'LineWidth', 1.5);
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2, 'Color', [0.5 0.8 0.5]);
    plot(graph_traj(1, :), graph_traj(2, :), 'r-', 'LineWidth', 2);
    if graph_lm.Count > 0
        lm_keys = cell2mat(keys(graph_lm));
        est_lm = zeros(length(lm_keys), 2);
        for i = 1:length(lm_keys)
            lm_pos = graph_lm(lm_keys(i));
            if isnumeric(lm_pos) && length(lm_pos) == 2
                est_lm(i, :) = lm_pos';
            end
        end
        plot(est_lm(:, 1), est_lm(:, 2), 'ro', 'MarkerSize', 6, 'LineWidth', 1.5);
    end
    legend('真实路标', '真实轨迹', 'Graph轨迹', 'Graph路标', 'Location', 'northeast');
    xlabel('X (m)'); ylabel('Y (m)');
    title(sprintf('Graph-SLAM (路标:%d, 误差:%.3fm)', graph_stats.n_landmarks, graph_stats.avg_error));
    xlim([-5, 50]); ylim([-5, 50]);
    
    % 子图4: FastSLAM轨迹与路标
    subplot(4, 3, 4); hold on; grid on; axis equal;
    plot(landmarks(:, 1), landmarks(:, 2), 'k*', 'MarkerSize', 8, 'LineWidth', 1.5);
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2, 'Color', [0.5 0.8 0.5]);
    plot(fastslam_traj(1, :), fastslam_traj(2, :), 'm-', 'LineWidth', 2);
    if ~isempty(fastslam_result.particles)
        [~, best_idx] = max([fastslam_result.particles.weight]);
        best_particle = fastslam_result.particles(best_idx);
        if ~isempty(best_particle.landmarks)
            n_lm = length(best_particle.landmarks);
            est_lm = zeros(n_lm, 2);
            for i = 1:n_lm
                est_lm(i, :) = best_particle.landmarks(i).mean';
            end
            plot(est_lm(:, 1), est_lm(:, 2), 'mo', 'MarkerSize', 6, 'LineWidth', 1.5);
        end
    end
    legend('真实路标', '真实轨迹', 'FastSLAM轨迹', 'FastSLAM路标', 'Location', 'northeast');
    xlabel('X (m)'); ylabel('Y (m)');
    title(sprintf('FastSLAM (路标:%d, 误差:%.3fm)', fastslam_stats.n_landmarks, fastslam_stats.avg_error));
    xlim([-5, 50]); ylim([-5, 50]);
    
    % 子图5: 误差对比
    subplot(4, 3, 5); hold on; grid on;
    plot(ekf_stats.errors, 'b-', 'LineWidth', 1.5);
    plot(graph_stats.errors, 'r-', 'LineWidth', 1.5);
    plot(fastslam_stats.errors, 'm-', 'LineWidth', 1.5);
    plot(grid_stats.errors, 'c-', 'LineWidth', 1.5);
    legend('EKF', 'Graph', 'FastSLAM', 'Grid+MCL', 'Location', 'best');
    xlabel('时间步'); ylabel('位置误差(m)');
    title('位置误差对比');
    
    % 子图6: 误差统计
    subplot(4, 3, 6);
    data = [ekf_stats.avg_error, graph_stats.avg_error, fastslam_stats.avg_error, grid_stats.avg_error;
            ekf_stats.max_error, graph_stats.max_error, fastslam_stats.max_error, grid_stats.max_error];
    b = bar(data);
    set(gca, 'XTickLabel', {'平均误差', '最大误差'});
    ylabel('误差(m)');
    legend(b, 'EKF', 'Graph', 'FastSLAM', 'Grid+MCL', 'Location', 'best');
    title('误差统计对比');
    grid on;
    
    % 子图7: 运行时间对比
    subplot(4, 3, 7);
    runtime_data = [ekf_stats.runtime, graph_stats.runtime, fastslam_stats.runtime, grid_stats.runtime];
    b = bar(runtime_data);
    b.FaceColor = 'flat';
    b.CData(1,:) = [0 0.4470 0.7410];  % 蓝色 - EKF
    b.CData(2,:) = [0.8500 0.3250 0.0980];  % 红色 - Graph
    b.CData(3,:) = [0.9290 0.6940 0.1250];  % 黄色 - FastSLAM
    b.CData(4,:) = [0 0.7 0.7];  % 青色 - Grid
    set(gca, 'XTickLabel', {'EKF', 'Graph', 'FastSLAM', 'Grid+MCL'});
    ylabel('运行时间 (秒)');
    title('算法运行时间对比');
    grid on;
    % 在柱子上方显示具体数值
    for i = 1:length(runtime_data)
        text(i, runtime_data(i), sprintf('%.2fs', runtime_data(i)), ...
             'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom', 'FontSize', 9);
    end
    
    % 子图8: Grid SLAM占据地图
    subplot(4, 3, 8); hold on; axis equal;
    % 转换log-odds到概率（与plot_grid_result相同的方法）
    prob_map = 1 - 1./(1 + exp(grid_map.occupancy));
    imagesc([0, grid_map.size], [0, grid_map.size], prob_map);  % 直接显示，occ_map(y,x)格式
    colormap(gca, flipud(gray));
    caxis([0, 1]);
    axis xy;  % 设置坐标轴方向：x向右，y向上
    c = colorbar;
    c.Label.String = '占据概率';
    % 绘制真实轨迹（直接使用真实坐标，无需转换）
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2.5);
    % 绘制估计轨迹（直接使用真实坐标，无需转换）
    plot(grid_traj(1, :), grid_traj(2, :), 'c-', 'LineWidth', 2);
    % 标记起点
    plot(true_traj(1, 1), true_traj(2, 1), 'go', 'MarkerSize', 10, 'LineWidth', 2, 'MarkerFaceColor', 'g');
    xlabel('X (m)'); ylabel('Y (m)');
    title(sprintf('Grid+MCL占据地图 (覆盖:%.1f%%)', grid_stats.map_coverage));
    xlim([0, 50]); ylim([0, 50]);
    
    % 子图9: 累积误差
    subplot(4, 3, 9); hold on; grid on;
    plot(cumsum(ekf_stats.errors), 'b-', 'LineWidth', 1.5);
    plot(cumsum(graph_stats.errors), 'r-', 'LineWidth', 1.5);
    plot(cumsum(fastslam_stats.errors), 'm-', 'LineWidth', 1.5);
    plot(cumsum(grid_stats.errors), 'c-', 'LineWidth', 1.5);
    legend('EKF', 'Graph', 'FastSLAM', 'Grid+MCL', 'Location', 'best');
    xlabel('时间步'); ylabel('累积误差(m)');
    title('累积误差对比');
    
    % 子图10: 算法效率对比（精度 vs 时间）
    subplot(4, 3, 10); hold on; grid on;
    % 计算效率指标（1/误差 - 越大越好）
    algorithms = {'EKF', 'Graph', 'FastSLAM', 'Grid+MCL'};
    avg_errors = [ekf_stats.avg_error, graph_stats.avg_error, fastslam_stats.avg_error, grid_stats.avg_error];
    runtimes = [ekf_stats.runtime, graph_stats.runtime, fastslam_stats.runtime, grid_stats.runtime];
    colors = [0 0.4470 0.7410; 0.8500 0.3250 0.0980; 0.9290 0.6940 0.1250; 0 0.7 0.7];
    
    for i = 1:4
        scatter(runtimes(i), avg_errors(i), 150, colors(i,:), 'filled', 'MarkerEdgeColor', 'k', 'LineWidth', 1.5);
        text(runtimes(i), avg_errors(i), ['  ' algorithms{i}], 'FontSize', 10, 'FontWeight', 'bold');
    end
    xlabel('运行时间 (秒)');
    ylabel('平均误差 (m)');
    title('算法效率对比（左下角=最优）');
    % 添加理想区域标注
    plot([min(runtimes), max(runtimes)], [mean(avg_errors), mean(avg_errors)], 'k--', 'LineWidth', 0.5);
    plot([mean(runtimes), mean(runtimes)], [min(avg_errors), max(avg_errors)], 'k--', 'LineWidth', 0.5);
    
    % 子图11: Grid SLAM轨迹详细对比
    subplot(4, 3, 11); hold on; grid on; axis equal;
    % 绘制真实轨迹（直接使用真实坐标，无需转换）
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2.5);
    % 绘制估计轨迹（直接使用真实坐标，无需转换）
    plot(grid_traj(1, :), grid_traj(2, :), 'c-', 'LineWidth', 2);
    % 标记起点和终点
    plot(true_traj(1, 1), true_traj(2, 1), 'go', 'MarkerSize', 10, 'LineWidth', 2, 'MarkerFaceColor', 'g');
    plot(true_traj(1, end), true_traj(2, end), 'gs', 'MarkerSize', 10, 'LineWidth', 2, 'MarkerFaceColor', 'g');
    plot(grid_traj(1, 1), grid_traj(2, 1), 'co', 'MarkerSize', 8, 'LineWidth', 2);
    plot(grid_traj(1, end), grid_traj(2, end), 'cs', 'MarkerSize', 8, 'LineWidth', 2);
    legend('真实轨迹', 'Grid+MCL', '真实起点', '真实终点', 'Grid起点', 'Grid终点', 'Location', 'northeast');
    xlabel('X (m)'); ylabel('Y (m)');
    title(sprintf('Grid+MCL (覆盖:%.1f%%, 误差:%.3fm)', grid_stats.map_coverage, grid_stats.avg_error));
    xlim([0, 50]); ylim([0, 50]);
    
    % 子图12: 综合性能雷达图（归一化）
    subplot(4, 3, 12);
    
    % 计算归一化指标（越大越好）
    avg_errors = [ekf_stats.avg_error, graph_stats.avg_error, fastslam_stats.avg_error, grid_stats.avg_error];
    runtimes = [ekf_stats.runtime, graph_stats.runtime, fastslam_stats.runtime, grid_stats.runtime];
    
    % 归一化（越大越好）：精度 = 1/误差, 速度 = 1/时间
    accuracy_scores = max(1./avg_errors) ./ (1./avg_errors);  % 归一化到最好的算法为1
    speed_scores = max(1./runtimes) ./ (1./runtimes);
    
    % 路标SLAM的路标数（Grid用地图覆盖率代替）
    landmark_scores = double([ekf_stats.n_landmarks, graph_stats.n_landmarks, fastslam_stats.n_landmarks, 0]);
    if max(landmark_scores) > 0
        landmark_scores = landmark_scores / max(landmark_scores);
    end
    landmark_scores(4) = double(grid_stats.map_coverage) / 100;  % Grid用覆盖率
    
    % 综合得分
    overall_scores = (accuracy_scores * 0.5 + speed_scores * 0.3 + landmark_scores * 0.2) * 100;
    
    b = bar(overall_scores);
    b.FaceColor = 'flat';
    b.CData(1,:) = [0 0.4470 0.7410];
    b.CData(2,:) = [0.8500 0.3250 0.0980];
    b.CData(3,:) = [0.9290 0.6940 0.1250];
    b.CData(4,:) = [0 0.7 0.7];
    set(gca, 'XTickLabel', {'EKF', 'Graph', 'FastSLAM', 'Grid+MCL'});
    ylabel('综合得分');
    title('综合性能评分 (精度50%+速度30%+建图20%)');
    ylim([0, 110]);
    grid on;
    % 显示得分
    for i = 1:4
        text(i, overall_scores(i), sprintf('%.1f', overall_scores(i)), ...
             'HorizontalAlignment', 'center', 'VerticalAlignment', 'bottom', 'FontSize', 9, 'FontWeight', 'bold');
    end
    
    sgtitle('SLAM算法全面对比分析', 'FontSize', 14, 'FontWeight', 'bold');
end

